#include <Servo.h>

const int trigPin = 8;        // 超声波Trig引脚
const int echoPin = 9;        // 超声波Echo引脚
const int MAX_DISTANCE = 50 + 92; // 50 + 学号后两位

Servo myservo;                
int currentState = 0;         // 0:90度, 1:45度, 2:135度

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo.attach(3);         
  myservo.write(90);          // 初始位置设为90度
  delay(1000);                
}

void loop() {
  // 超声波测距
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  distance = 50;  //此处直接用常量替代，进行模拟测试

  Serial.print("状态: ");
  Serial.print(currentState);
  Serial.print(", 距离: ");
  Serial.print(distance);
  Serial.println(" cm");

  // 舵机控制逻辑
  if (distance > MAX_DISTANCE) {
    if (currentState != 0) {  // 若距离超过阈值且不在初始状态
      myservo.write(90);
      currentState = 0;
      delay(500);             // 等待舵机回转
    }
  } else {
    switch (currentState) {
      case 0:                 // 初始状态，左转45度
        myservo.write(45);
        currentState = 1;
        delay(500);           // 等待舵机转动
        break;
      case 1:                 // 左转状态，检测是否仍需右转
        if (distance <= MAX_DISTANCE) {
          myservo.write(135); // 右转90度至135度
          currentState = 2;
        } else {
          myservo.write(90);  // 返回初始位置
          currentState = 0;
        }
        delay(500);
        break;
      case 2:                 // 右转状态，检测是否返回
        if (distance <= MAX_DISTANCE) {
          myservo.write(90);  // 返回初始位置
          currentState = 0;
          delay(500);
        }
        break;
    }
  }
  delay(1000);                 
}
